#include <QCoreApplication>

#include "urcontrolstamp.hpp"

#if 1
int main(int argc, char *argv[])
{
    if(argc!=10)
    {
        std::cout<<"parameters error\n";
        return 1;
    }

    cv::Vec2d cornerPixel[4];
    double v=0.1;

    std::istringstream(argv[1])>>cornerPixel[0][0];
    std::istringstream(argv[2])>>cornerPixel[0][1];
    std::istringstream(argv[3])>>cornerPixel[1][0];
    std::istringstream(argv[4])>>cornerPixel[1][1];
    std::istringstream(argv[5])>>cornerPixel[2][0];
    std::istringstream(argv[6])>>cornerPixel[2][1];
    std::istringstream(argv[7])>>cornerPixel[3][0];
    std::istringstream(argv[8])>>cornerPixel[3][1];
    std::istringstream(argv[9])>>v;

    URControlStamp urcs;
     //urcs.TouchRectangleCenter(cornerPixel, cv::Vec3d(0,0,0.21), v);

    // void ForceTouchRectangleCenter(cv::Vec2d rectangleCorner[4], cv::Vec3d fingerXYZ, double v=0.2)
    // cv:Vec2d => cornerPixel
    // cv::Vec3d => (0,0,0.21)
    urcs.ForceTouchRectangleCenter(cornerPixel, cv::Vec3d(0,0,0.21), v=0.2); // San 20170810 Activate grab_cap.urp


    //urcs.SetPose(-0.0425296,-0.277517,0.388848,-1.27595,2.85684,-0.0391458, v);


    //urcs.LoadAndPlay("RG2_Testing/open_30");


//    std::this_thread::sleep_for(std::chrono::seconds(1));

    return 0;
}
#endif

#if 0

#include "urcontrolplane.hpp"

int main(int argc, char *argv[])
{
URControlPlane urcp;
urcp.test();

    return 1;

    URControlStamp urcs;
    double aa[6];
    for(int i=0;i<8;i++)
    {
        urcs.GetPose(aa[0],aa[1],aa[2],aa[3],aa[4],aa[5]);
    }
    return 0;
}
#endif


// RG2_Testing/open_30 Testing
#if 0
int main(int argc, char *argv[])
{
    URControlStamp urcs;
    //    urcs.ForceTouch();
    //    urcs.Touch();
//    urcs.Test();

    for(int i=0;i<4;i++)
    {
        //urcs.LoadAndPlay2("RG2_Testing/open30_1");
        urcs.set_gripper(30);
        std::this_thread::sleep_for(std::chrono::milliseconds(5000));

    //urcs.LoadAndPlay2("RG2_Testing/close_1");
        urcs.set_gripper(0);
    std::this_thread::sleep_for(std::chrono::milliseconds(5000));
    }
    return 0;
}
#endif
